//!Includes

#include "MainApplication.h"

//!Global Functions



namespace apps
{


/**
 * class MainApplication
 *
 *
 * PROJECT:
 *
 */

//!Constructors/Destructors

/**
 * Empty Constructor
 */
MainApplication::MainApplication (int argc,char **argv)
{
	m_pControlRobot=new tasks::ControlRobotTask(argc,argv);
	initAttributes ( );
}

/**
 * Empty Destructor
 */
MainApplication::~MainApplication ( )
{
	delete m_pControlRobot;
	delete m_pReport;
	delete m_pTraking;
}

//!Public Accessor Methods



//!Public Methods


void MainApplication::Execute ( )
{

	m_pControlRobot->SetArClient(&m_Client);
	m_pControlRobot->ConectWithRobot();
	m_pTraking->SetArClient(&m_Client);

	m_pTraking->CreateWindow(std::string("result"));
	m_pTraking->CreateWindow(std::string("source"));

	m_pTraking->SetReference(NEWTARGET);
	m_pTraking->ShowSourceImage(0,std::string("source"));

	while(1)
	{
		AchieveTarget ();
		AproximationAndReturn ();
		while(!SearchTarget ());
		AchieveTarget ();
		AproximationAndReturn ();
		while(!SearchTarget ());

	}

}

//!Protected Methods



//!Private Methods
void MainApplication::AchieveTarget ( )
{



	while(!m_pControlRobot->GetAchieveTarget()&&m_iCount<500){

		m_pTraking->GetImage(m_iModeGetImage);

		m_pTraking->FindTarget();
		m_pTraking->FindTargetPoints();
		m_pTraking->FillTarget(m_iModeControl);

		if(m_pTraking->m_bFillVect&&m_pTraking->m_bNewVect)
		{
			m_TargetVector=m_pTraking->GetTargetVector();
			PushVector (m_iModeControl);
			m_iCount++;
			m_pControlRobot->SetTargetVector(m_TargetVector);
			m_pControlRobot->LostTarget(false);
			m_pControlRobot->Execute(m_iModeControl);
			m_TargetVector.clear();
		}
		else if(m_pTraking->m_bFillVect)
		{
			m_TargetVector=m_pTraking->GetTargetVector();
			PushVector (m_iModeControl);
			m_iCount++;
			m_pControlRobot->SetTargetVector(m_TargetVector);
			m_pControlRobot->LostTarget(true);
			m_pControlRobot->Execute(m_iModeControl);
			m_TargetVector.clear();
		}

		m_pTraking->ShowSourceImage(5,std::string("source"));
		m_pTraking->ShowRoiImage(5,std::string("result"));
		m_pTraking->ReleaseDestinImage();
		m_pTraking->ReleaseRoiImage();

		if(m_iModeGetImage==FROMSERVER)
		{
			m_pTraking->ReleaseSourceImage();
		}

		if(m_pControlRobot->GetAlign()&&m_bAlign==false)
		{
			printf("Align\n");
			m_pTraking->ClearBuffer();
			m_bAlign=true;
		}

	}




	m_pReport->SetSignal(m_VectorSignal1);
	m_pReport->SetCount(m_iCount);
	m_pReport->CreateSignal("signal alfa");
	m_VectorSignal1.clear();

	m_pReport->SetSignal(m_VectorSignal2);
	m_pReport->SetCount(m_iCount);
	m_pReport->CreateSignal("signal P");
	m_VectorSignal2.clear();

	m_pReport->SetSignal(m_VectorSignal3);
	m_pReport->SetCount(m_iCount);
	m_pReport->CreateSignal("signal beta");
	m_VectorSignal3.clear();
	m_pReport->Execute();

	m_pControlRobot->SetAchieveTarget(false);
	m_iCount=0;
	m_pControlRobot->SetAlign(false);
	m_bAlign=false;

}

bool MainApplication::SearchTarget ( )
{
	m_pTraking->GetImage(m_iModeGetImage);
	m_pTraking->FindTarget();
	m_pTraking->FindTargetPoints();
	m_pTraking->FillTarget(m_iModeControl);
	m_TargetVector=m_pTraking->GetTargetVector();
	m_pControlRobot->SetTargetVector(m_TargetVector);
	if(!m_pTraking->m_bNewVect)
	{
		m_pControlRobot->SetHeading(-15);
		ArUtil::sleep(2000);
	}
	m_TargetVector.clear();

	m_pTraking->ShowSourceImage(5,std::string("source"));
	m_pTraking->ShowRoiImage(5,std::string("result"));
	m_pTraking->ReleaseDestinImage();
	m_pTraking->ReleaseRoiImage();

	if(m_iModeGetImage==FROMSERVER)
	{
		m_pTraking->ReleaseSourceImage();
	}


	return m_pTraking->m_bNewVect;
}

void MainApplication::PushVector (int val)
{
	if(val==PBVS)
	{
		m_VectorSignal1.push_back(cvPoint(m_iCount,m_TargetVector.back().GetAlfa()));
		m_VectorSignal2.push_back(cvPoint(m_iCount,m_TargetVector.back().GetP()/100));
		m_VectorSignal3.push_back(cvPoint(m_iCount,m_TargetVector.back().GetBeta()));
	}
	else if(val==IBVS)
	{
		m_VectorSignal1.push_back(cvPoint(m_iCount,m_TargetVector.back().GetDiffCenterX()));
		m_VectorSignal2.push_back(cvPoint(m_iCount,m_TargetVector.back().GetDiffDiagonal()));
		if(m_TargetVector.back().GetDiffEstOrientation()>0)
		{
			m_VectorSignal3.push_back(cvPoint(m_iCount,std::abs(m_TargetVector.back().GetDiffOrientation())));
		}
		else
		{
			m_VectorSignal3.push_back(cvPoint(m_iCount,-std::abs(m_TargetVector.back().GetDiffOrientation())));
		}

	}

}

void MainApplication::AproximationAndReturn ( )
{
	m_pControlRobot->Move(700);
	ArUtil::sleep(10000);
	m_pControlRobot->Move(-400);
	ArUtil::sleep(5000);
	m_TargetVector=m_pTraking->GetTargetVector();
	m_pControlRobot->SetTargetVector(m_TargetVector);
	m_pControlRobot->SetHeading(-20);
	ArUtil::sleep(5000);
	m_TargetVector.clear();
}

void MainApplication::initAttributes ( )
{
	m_pReport=new tasks::ReportTask;
	m_pTraking=new tasks::TrackingTask;
	m_pClient=&m_Client;
	m_iCount=0;
	m_iModeControl=IBVS;
	m_iModeGetImage=FROMSERVER;
	m_bAlign=false;
}
};
